#include "testApp.h"
#include "globals.h"

//--------------------------------------------------------------
void testApp::setup(){

    ofLogNotice() << "in setup..." << endl;


    ofSetFrameRate(frameRate);

    ofLogNotice() << "making connection with kinect..." << endl;
    // enable depth->video image calibration
	kinect.setRegistration(true);
	kinect.init();
	kinect.open();

	// print the intrinsic IR sensor values
	if(kinect.isConnected()) {
		ofLogNotice() << "sensor-emitter dist: " << kinect.getSensorEmitterDistance() << "cm";
		ofLogNotice() << "sensor-camera dist:  " << kinect.getSensorCameraDistance() << "cm";
		ofLogNotice() << "zero plane pixel size: " << kinect.getZeroPlanePixelSize() << "mm";
		ofLogNotice() << "zero plane dist: " << kinect.getZeroPlaneDistance() << "mm";
	}

	//allocating space for the images
	grayImage.allocate(kinect.width , kinect.height);
    colorImage.allocate(kinect.width , kinect.height);

    //polygon detection stuff
    //setting up containers and getting as a greyscale image
    detectImage.loadImage("FindingContours.png");
    detectHeight = detectImage.height;
    detectWidth = detectImage.width;

    detectColImg.allocate(detectWidth, detectHeight);
    detectGSImg.allocate(detectWidth, detectHeight);
    detectColImg.setFromPixels(detectImage.getPixels(),detectWidth, detectHeight);
    detectGSImg = detectColImg;

    //thresholding (increasing contrast) the grayscale image to get better results
    //detectGSImg.threshold(30);

    // find contours which are between the size of 20 pixels and 1/3 the w*h pixels.
    // also, find holes is set to true so we will get interior contours as well....
    contourFinder.findContours(detectGSImg, 50, (detectWidth*detectHeight)/5, 10, false,true);

    //go through the blobs in the contour finder to figure out what shape they are.
    cout << "num blobs found:" << contourFinder.nBlobs<<endl;


    for (int i = 0; i < contourFinder.nBlobs; i++){
        cout << "blob ["<<i<<"] numVertices:"<<contourFinder.blobs[i].nPts<<endl;
        cout << "blob ["<<i<<"] perimeter:"<<contourFinder.blobs[i].length<<endl;
        //cout << "blob ["<<i<<"] centroid:"<<contourFinder.blobs[i].length<<endl;
        cout << endl;
    }

}

//--------------------------------------------------------------
void testApp::update(){
    //cout << "in update..."<< endl;
    ofBackground(darkGray);
	kinect.update();

	// there is a new frame and we are connected
	if(kinect.isFrameNew()) {
        //cout << "New Kinect Frame, need to update"<<endl;

        grayImage.setFromPixels(kinect.getDepthPixels(), kinect.width, kinect.height);
        colorImage.setFromPixels(kinect.getPixels(),kinect.width, kinect.height);

		int numPixels = grayImage.getWidth()* grayImage.getHeight();
        unsigned char * grayPixList = grayImage.getPixels();
        unsigned char * colorPixList = colorImage.getPixels();

        int rVal, gVal, bVal;

        rVal = (int)colorPixList[0];
        gVal = (int)colorPixList[1];
        bVal = (int)colorPixList[2];

        ofColor pixCol(rVal,gVal,bVal);
        ofBackground(pixCol);

	}
}

//--------------------------------------------------------------
void testApp::draw(){
    //cout << "in draw..."<< endl;
        //grayImage.draw(10, 320, 400, 300);
    //colorImage.draw(650, 10, 400, 300);

    ofSetColor(white);
    detectImage.draw(0,0,640,327);
    contourFinder.draw(0,0);

    for (int i = 0; i < contourFinder.nBlobs; i++){
        blobCentroid = contourFinder.blobs[i].centroid;
        cout << "blob ["<<i<<"] centroid:"<<blobCentroid.x<<","<<blobCentroid.y<<","<<blobCentroid.z<<endl;

        ofSetColor(blue);
        ofCircle(blobCentroid.x, blobCentroid.y, blobCentroid.z, 5);

        vBlobVertices = contourFinder.blobs[i].pts;
        for (int i=0; i< vBlobVertices.size(); i++){
            ofSetColor(blue);
            ofCircle(vBlobVertices.at(i).x, vBlobVertices.at(i).y, vBlobVertices.at(i).z, 1);
        }
    }

    //for (int i = 0; i < contourFinder.nBlobs; i++){
       // contourFinder.blobs[i].draw(5,10);
		// draw over the centroid if the blob is a hole
		/*ofSetColor(255);
		if(contourFinder.blobs[i].hole){
			ofDrawBitmapString("hole",
				contourFinder.blobs[i].boundingRect.getCenter().x + 5,
				contourFinder.blobs[i].boundingRect.getCenter().y + 10);
		}*/
    //}

}

//--------------------------------------------------------------
void testApp::keyPressed(int key){}
void testApp::keyReleased(int key){}
void testApp::mouseMoved(int x, int y ){}
void testApp::mouseDragged(int x, int y, int button){}
void testApp::mousePressed(int x, int y, int button){}
void testApp::mouseReleased(int x, int y, int button){}
void testApp::windowResized(int w, int h){}
void testApp::gotMessage(ofMessage msg){}
void testApp::dragEvent(ofDragInfo dragInfo){}
